/*
 * File:   serialUltrasonic.cpp
 * Author: samcrow
 *
 * Created on March 4, 2012, 4:58 PM
 */

#include "serialUltrasonic.h"


using namespace std;

SerialUltrasonic::SerialUltrasonic(std::string port, int baudRate) {
    portID = openPort(port, baudRate);

	cout << "Serial port identifier: " << portID << endl;
}

int SerialUltrasonic::getDistance(){
	string distanceString = getDistanceString();

    int distance = atoi(distanceString.c_str());//Convert that to an integer

    return distance;
}

string SerialUltrasonic::getDistanceString(){
	char buffer[256];
    readUntil(portID, buffer, '\r');
    string distanceString(buffer);

	cout << "Received raw data: " << distanceString << endl;

	if(distanceString.length() < 4){
		cout << "No valid data received from the ultrasonic sensor" << endl;
		return "-1";
	}

    distanceString = distanceString.substr(1, 3);//Get the three numerical digits starting at position 2

	if(!isNumeric(distanceString)){

		//See if it is in the form "<digit><digit>\n"
		distanceString = distanceString.substr(0, 3);//Get the first two characters, hopefully just digits
		if(isNumeric(distanceString)){
			return distanceString;//If it's valid, return it
		}

		cout << "Data received (\"" << distanceString << "\") was not in valid numerical form." << endl;

		//Try again (Recursion!). Hopefully this will never actually overflow the stack in competition.
		usleep(.02 * 1000 * 1000);//Wait .02 seconds for the sensor to send new data
		return getDistanceString();
	}

	return distanceString;
}

int SerialUltrasonic::readUntil(int fd, char* buf, char until){
    /* Contents of this function: Created 5 December 2006
     * Copyleft (c) 2006, Tod E. Kurt, tod@todbot.com
     * http://todbot.com/blog/
     */
    char b[1];
    int i=0;
    do {
        int n = read(fd, b, 1);  // read a char at a time
        if( n==-1) return -1;    // couldn't read
        if( n==0 ) {
            usleep( 10 * 1000 ); // wait 10 msec try again
            continue;
        }
        buf[i] = b[0]; i++;
    } while( b[0] != until );

    buf[i] = 0;  // null terminate the string
    return 0;
}

int SerialUltrasonic::openPort(string name, int baud){
    const char* serialport = name.c_str();
    /* Contents of this function: Created 5 December 2006
     * Copyleft (c) 2006, Tod E. Kurt, tod@todbot.com
     * http://todbot.com/blog/
     */

    struct termios toptions;
    int fd;

    //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n",
    //        serialport,baud);

    fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd == -1)  {
        perror("init_serialport: Unable to open port ");
        return -1;
    }

    if (tcgetattr(fd, &toptions) < 0) {
        perror("init_serialport: Couldn't get term attributes");
        return -1;
    }
    speed_t brate = baud; // let you override switch below if needed
    switch(baud) {
    case 4800:   brate=B4800;   break;
    case 9600:   brate=B9600;   break;
#ifdef B14400
    case 14400:  brate=B14400;  break;
#endif
    case 19200:  brate=B19200;  break;
#ifdef B28800
    case 28800:  brate=B28800;  break;
#endif
    case 38400:  brate=B38400;  break;
    case 57600:  brate=B57600;  break;
    case 115200: brate=B115200; break;
    }
    cfsetispeed(&toptions, brate);
    cfsetospeed(&toptions, brate);

    // 8N1
    toptions.c_cflag &= ~PARENB;
    toptions.c_cflag &= ~CSTOPB;
    toptions.c_cflag &= ~CSIZE;
    toptions.c_cflag |= CS8;
    // no flow control
    toptions.c_cflag &= ~CRTSCTS;

    toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
    toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

    toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
    toptions.c_oflag &= ~OPOST; // make raw

    // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
    toptions.c_cc[VMIN]  = 0;
    toptions.c_cc[VTIME] = 20;

    if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
        perror("init_serialport: Couldn't set term attributes");
        return -1;
    }

    return fd;
}

bool SerialUltrasonic::isNumeric(std::string input){
	const char digits[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9'};
	const int numDigits = sizeof(digits) / sizeof(char);//Get the actual number of digits in the array

	bool isNumeric = true;//If the input string is numeric. This is set to true so that any non-digit character can set it to false

	const int length = input.length();
	for(int i = 0; i < length; i++){//Iterate over every character of the string
		const char thisCharacter = input.at(i);//Get the character from this position of the string

		bool isDigit = false;//If this character is a numerical digit

		for(int j = 0; j < numDigits; j++){//Iterate over every digit and see if one matches this character
			if(thisCharacter == digits[j]){
				isDigit = true;
			}
		}

		if(!isDigit){//If this character is not a digit, indicate that the entire string is not numerical
			isNumeric = false;
		}
	}

	return isNumeric;
}

SerialUltrasonic::~SerialUltrasonic() {
    close(portID);
}
